#include <fstream>

#include "imr_converter.h"

using namespace std;

void IMRConverter::setIMRHeader()
{
	strcpy(_imr_header._sz_header,"$IMURAW\0");
	_imr_header._is_intel_or_motorola = 0; // use intel little endian by default
	_imr_header._version_number = 8.80;	// default
	_imr_header._delta_theta = 0;	// output is angular rate rather than increments,thus set to 0
	_imr_header._delta_velocity = 0;	// is the same as the above, just take care
	_imr_header._data_rate_hz = 100;
	_imr_header._gyro_scale_factor = 0.0001;
	_imr_header._accel_scale_factor = 0.0001;
	_imr_header._utc_or_gpstime = 0;
	_imr_header._rcvtime_or_corrtime = 0;
	_imr_header._time_tag_bias = 0.0;
	strcpy(_imr_header._sz_imu_name, "INS-D\0");
	strcpy(_imr_header._sz_program_name, "Unknown\0");
	_imr_header._leverarm_valid = false;
	_imr_header._x_offset = 0;
	_imr_header._y_offset = 0;
	_imr_header._z_offset = 0;
}

void IMRConverter::convertToIMR(const string& imu_filename, const string& imr_filename)
{
	ifstream ifs(imu_filename, ios::binary);
	ofstream ofs(imr_filename, ios::binary);
	double time;
	int32_t gyro[3];
	int32_t accel[3];
	if (!ifs.is_open() && !ofs.is_open())
		return;
	//ofs.write((char*)&_imr_header, sizeof(_imr_header));	//write header
	ofs.write((char*)_imr_header._sz_header, sizeof(_imr_header._sz_header));
	ofs.write((char*)&_imr_header._is_intel_or_motorola, sizeof(_imr_header._is_intel_or_motorola));
	ofs.write((char*)&_imr_header._version_number, sizeof(_imr_header._version_number));
	ofs.write((char*)&_imr_header._delta_theta, sizeof(_imr_header._delta_theta));
	ofs.write((char*)&_imr_header._delta_velocity, sizeof(_imr_header._delta_velocity));
	ofs.write((char*)&_imr_header._data_rate_hz, sizeof(_imr_header._data_rate_hz));
	ofs.write((char*)&_imr_header._gyro_scale_factor, sizeof(_imr_header._gyro_scale_factor));
	ofs.write((char*)&_imr_header._accel_scale_factor, sizeof(_imr_header._accel_scale_factor));
	ofs.write((char*)&_imr_header._utc_or_gpstime, sizeof(_imr_header._utc_or_gpstime));
	ofs.write((char*)&_imr_header._rcvtime_or_corrtime, sizeof(_imr_header._rcvtime_or_corrtime));

	ofs.write((char*)&_imr_header._time_tag_bias, sizeof(_imr_header._time_tag_bias));
	ofs.write((char*)_imr_header._sz_imu_name, sizeof(_imr_header._sz_imu_name));
	ofs.write((char*)_imr_header._reserverd1, sizeof(_imr_header._reserverd1));
	ofs.write((char*)_imr_header._sz_program_name, sizeof(_imr_header._sz_program_name));
	ofs.write((char*)&_imr_header._t_create, sizeof(_imr_header._t_create));
	ofs.write((char*)&_imr_header._leverarm_valid, sizeof(_imr_header._leverarm_valid));
	ofs.write((char*)&_imr_header._x_offset, sizeof(_imr_header._x_offset));
	ofs.write((char*)&_imr_header._y_offset, sizeof(_imr_header._y_offset));
	ofs.write((char*)&_imr_header._z_offset, sizeof(_imr_header._z_offset));
	ofs.write((char*)_imr_header._reserved, sizeof(_imr_header._reserved));
	int a = ofs.tellp();
	while (!ifs.eof())
	{	
		ifs.read((char*)&time, sizeof(time));
		ifs.read((char*)gyro, sizeof(gyro));
		ifs.read((char*)accel, sizeof(accel));

		ofs.write((char*)&time, sizeof(time));
		ofs.write((char*)gyro, sizeof(gyro));
		ofs.write((char*)accel, sizeof(accel));
	}
	ifs.close();
	ofs.close();
}
